/*
 * controller.c
 *
 *  Created on: Oct 11, 2013
 *      Author: Manuel
 */

#define MIN_DC_SERVO	59.0		// = 5% at RES_CALC
#define MAX_DC_SERVO	93.0		// = 10% at RES_CALC  88
#define CENTER_DC_SERVO 72.0

#define Kp 0.4		//kp=0.4 Kd=0.15
#define Kd 0
#define t  0.01		//time 10 tks


extern volatile short int Error;
volatile short int Last_Error=0;
extern volatile float  LM,RM;

extern volatile char K;
extern volatile char center;

void P_Gain_Servo()
{
	Last_Error=Error;	//derivade error 
	Error=64-center;	//Error detect e=Ref-actual	
	
	K=(Kp*(Error)) - ((Kd)*(Error-Last_Error)) + CENTER_DC_SERVO ;		//K_Gain (Kd*(Error-Last_Error))

}


void DC_Motors()
{
	if(center<30)
	{
		RM=0;
		LM=60;
		
	}
	
	else if(center>70)
	{
		LM=0;
		RM=60;
	}
	
	else
	{
		LM=40;
		RM=40;
	}
	
	
}

